class_name Boid extends QuadTreeObj

const MAX_SPEED = 200.0 # 最大速度
const MAX_FORCE = 10.0 # 最大加速度
const SEE_RADIUS = 50.0 # 视野半径
const SEPARATION = 40.0 # 最小间隔距离

const RAY_LEN = 150 # 射线长度

var velocity = Vector2.ZERO
var traceTarget: Vector2 # 追踪目标
# 用于直接访问底层物理空间，高效地处理物理交互
var deepSpace: PhysicsDirectSpaceState2D

var gatherW = 1.0 # 聚集权重
var alineW = 1.3 # 对齐权重
var apartW = 1.2 # 分离权重
var traceW = 0.5 # 追踪权重
var avoidW = 0 # 避让权重


func _ready() -> void:
	velocity = Vector2(randf() * MAX_SPEED, randf() * MAX_SPEED)
	rotation = velocity.angle()
	deepSpace = get_world_2d().direct_space_state

# 计算移动加速度
func steerTowards(vel: Vector2) -> Vector2:
	# 当前方向-->目标方向
	var steer = vel.normalized() * MAX_SPEED - velocity
	# 限制转向向量的大小，防止突然的大幅度转向
	return steer.limit_length(MAX_FORCE)

# 聚集的方向
func _gather(neighbors) -> Vector2:
	var sum = Vector2.ZERO
	var count = 0
	for other in neighbors:
		if other == self: continue

		var distance = position.distance_to(other.position)
		# 累计视野半径内的鸟儿的位置，以方便计算群体的重心
		if distance > 0 and distance < SEE_RADIUS:
			sum += other.position
			count += 1

	# 获得加速度向量
	if count > 0:
		sum /= count
		sum = steerTowards(sum - position)
	return sum
	
# 计算对齐的方向
func _aline(neighbors) -> Vector2:
	var sum = Vector2.ZERO
	var count = 0
	for other in neighbors:
		if other == self: continue

		var distance = position.distance_to(other.position)
		# 累计视野半径内的鸟儿的速度，以方便计算平均速度
		if distance > 0 and distance < SEE_RADIUS:
			sum += other.velocity
			count += 1

	# 获得加速度向量
	if count > 0:
		sum /= count
		sum = steerTowards(sum)

	return sum

# 计算分离的方向
func _apart(neighbors) -> Vector2:
	var sum = Vector2.ZERO
	var count = 0
	for other in neighbors:
		if other == self: continue

		var distance = position.distance_to(other.position)
		# 累计小于最小间隔的邻居与我形成的向量方向diff
		# 使用distance进行缩放，距离越近的邻居对分离的影响则越大
		if distance > 0 and distance < SEPARATION:
			var diff = (position - other.position).normalized()/distance
			sum += diff
			count += 1

		# 获得加速度向量
	if count > 0:
		sum /= count
		sum = steerTowards(sum)

	return sum

# 追踪方向
func _trace() -> Vector2:
	if !traceTarget: return Vector2.ZERO
	return steerTowards(traceTarget - position)

# 避让方向
func _avoid() -> Vector2:
	var steer = Vector2.ZERO
	var target = global_position + velocity.normalized() * RAY_LEN
	# 创建检测射线
	var ray = PhysicsRayQueryParameters2D.create(global_position, target)
	var collide = deepSpace.intersect_ray(ray)

	# 检测到障碍物
	if collide.size():
		# 计算避让角度
		var dir = _avoidDir()
		steer = steerTowards(dir)
	return steer

func _avoidDir() -> Vector2:
	var curDir = velocity.normalized()
	var limit = 10
	# 循环5次，分别正向和逆向各检测5次，最大转向PI/4
	for i in range(int(limit / 2.0)):
		for j in range(2):
			# 三元表达 d= j==0?1:-1
			var d = 1 if j == 0 else -1
			var angle = d * (PI / limit / 2) * i
			var dir = curDir.rotated(angle)
			var ray = PhysicsRayQueryParameters2D.create(global_position, global_position + dir * RAY_LEN)
			var collide = deepSpace.intersect_ray(ray)

			if collide.size() == 0: return dir
		
	return curDir

# 应用三个加速度向量到实体速度
func apply(delta: float, qt: QuadTree) -> void:
	var nearby = []
	# 搜索视野范围内的邻居
	var rect = Rect2(position - Vector2(SEE_RADIUS, SEE_RADIUS), Vector2(SEE_RADIUS * 2, SEE_RADIUS * 2))
	qt.query(rect, nearby)

	var gather = _gather(nearby) * gatherW
	var apart = _apart(nearby) * apartW
	var aline = _aline(nearby) * alineW
	var trace = _trace() * traceW
	var avoid = _avoid() * avoidW

	var accelaration = gather + aline + apart + trace + avoid
	velocity += accelaration
	velocity = velocity.limit_length(MAX_SPEED)
	position += velocity * delta
	# 插值旋转
	if velocity.length() > 0:
		rotation = lerp_angle(rotation, velocity.angle(), PI * delta)

	wrapScreen()
	# 更新四叉树中的对象
	qt.update(self)

# 限制移动范围为屏幕
func wrapScreen() -> void:
	var limitSize = get_viewport_rect().size
	position.x = wrapf(position.x, 0, limitSize.x)
	position.y = wrapf(position.y, 0, limitSize.y)

func setTarget(target: Vector2) -> void:
	traceTarget = target